A faster algorithm for calculating the inverse kinematics of a general 6R manipulator for robot real time control

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

14th

World Congress ofTFAC

Copyright

to

1999 IFAC

B-ld-lO.,S

14th Triennial World Congress, Beijing, P.R.

China

A

FASTER

ALGORITHM

FOR

CALCULATING

mE

INVERSE

KINEMATICS

OF

A

GENERAL

6R

MANIPULATOR

FOR

ROBOT

REAL

TIME

CONTROL.

Sebastian

R.

G6mez

*,

Jose

A.

Cerrada

*,

Vicente

Feliu

**

*

DIEEC

ETSII

UNED

Ciudad

Universitaria

sn

28040

Madrid, Spain.

(sgomez,

jcerrada

@ ieec.uned.es)

**

ETS

INDUSTRIALES

Campus

Universitario sn 13071 Ciudad Real, Spain

(vfeliu@ind-cr.uclm.es)

Abstract:

This

article

presents

a

new

faster

algorithm

for

the

real

time

calculation

of

the

inverse kinematics

of

general,

6R,

six

revolute

jointed,

manipulators.

It

solves

the

inverse

kinematics

problem

towards a

set

of

non-

linear

algebraic

equations

that

are

linearized

obtaining

a 16

u

,

degree

polynomial

in the

tangent

of

the half-angle

of

a revolute

joint.

Elimination

methods

were

originally presented by

Lee

and

Liang.

The

same

method

was also

derived

by

Raghavan

and

Roth,

although

the first solution with

real

time

performance

was presented

by

Manocha

and

Canny.

The

method

proposed

here

is faster

than

the

one

from

the last authors and also

avoids

the

singularities

that

could

appear

in theirs.

The

aim

of

this algorithm

is

to

find

one

of

the 16 possible solutions

in

order

to control a general

6R

manipulator

in real time

and

perform

path

following.

Using

Rhaghavan

and

Roth

method,

the

coefficients

of

the

16-degree

polynomial

are

calculated

in real

time

by

evaluating

the symbolic

determinant

that

generates

this

polynomial

in 17 different points.

The

final solution is

reached

by

a

root

solver,

beginning

the

search

in

the

last

instant

root.

The

average

performance

time

of

the

algorithm

is 25 ms in a

PC

Pentium

at

66

MHz.

This

allows

real

time

generation

of

the

references

for

the

local controllers

of

the actuators

of

the

six

joints

that

can

be

included

in

1999IFAC

Keywords: Inverse Kinematics,

Efficient

Algorithms,

Real

Time

Control,

Robot

Control,

Robot

Modeling.

Copyright

1.

INTRODUCTION

The

inverse

kinematics

problem

is

fundamental

in

the

design

and

control

of

robot

manipulators.

The

trajectories

of

robots

are

usually

specified

in terms

of

the

end

-

effect

pose,

position

and orientation, in

Cartesian

space, while its

motion

control

is specified

in

terms

of

joint

angles

in

joint

space.

The

inverse

kinematics

gives

the

required

joint

angles

to

reach

one

desired

pose

with the

end

- effector.

If

a real

time

control

is required, in real

time

path

following,

for

instance,

the

inverse

kinematics

must

generate

joint

angle references within

the

control

period,

which

implies

a

very

low

time

execution

for the

inverse kinematics algorithm.

Such

control

period

is

of

the

order

of

0.01 seconds.

Then

a critical

issue

in

robot

control

is

to

design

algorithms

to

carry

out

the

inverse

kinematics·

in

a

time

of

that

order

of

magnitude.

The

more

joints

the

robot

has,

the bigger

is the

complexity

of

the inverse kinematics.

The

most

interesting case

has

been

that

of

serial

manipulators

with six

degrees

of

freedom,

(DOF).

With 6

DOF,

any

pose

in

the

space

can

be

reached.

The

complexity

of

inverse

kinematics

in

a general 6

DOF

manipulator

is a function

of

its

geometry.

This

complexity

can

be

avoided

by

'adopting

a

"closed

form"

geometry

that

is

when

three

consecutive

axes

intersect in a

common

point

or

they are parallel.

The

problems

arise

in

the general case,

when

no

such

restrictions

are

applied

to

the

manipulator.

In

fact

most

of

6-DOF

industrial manipulators

use

"closed

form"

configurations

to

avoid

calculating the inverse

kinematics

of

a general geometry.

There

are

robot

applications

where

the general

robot

geometry

is

more

efficient

than

the

before

"closed

forms".

For

example,

when

slave teleoperators

are

designed.

The

inverse

kinematics

problem

of

general

configurations has

been

studied

during

the

last three

decades, particularly

for

6

DOF

manipulators.

The

first

work

was

that

of

Pieper

(1968),

where

he

presented

the

first

inverse

kinematics

solution

based

on

iterative

numerical

techniques.

He

also

presented

the closed

form

solution.t'or a special geometry.

ISBN:

008

0432484

833

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

Roth

et al.

(1973)

showed

that

this

problem

has

an

upper

bound

of

32 solutions,

that

is,

any

6

DOF

manipulator

could

reach

a goal position with his end

- effector in 32 distinct configurations.

The

first

constructive

solution

was

given

by

Albala

and

Angeles

(1979)

in

the

form

of

a

determinant

of

12x12

matrix

whose

entries were quartic

polynomials

in

the

tangent

of

the

half-angle

of

one

of

the

joint

variables.

The

was

given

by

Duffy

and

Crane

(1980)

who

provided

a

32-degree

polynomial

in the

tangent

of

the

half-angle

using

dyalitic elimination.

This

polynomial

always

presented

extraneous

or

false

solutions

to

the

desired

values.

Tsai

and

Morgan,

(1985)

brought

down

the

number

of

solutions

to

16.

They

cast

the

problem

in a

set

of

eight

second-degree

equations.

The

first

conclusive

proof

of

the

existence

of

16

solutions was

given

by

Lee

and

Liang

(1988),

who

derived

a 16-

degree

polynomial

in

the

tangent

of

the

half- angle.

Raghavan

and Roth,

(1990),

derived

this

16-degree

polynomial

using

dyalitic elimination.

Manocha

and

Canny,

(1992)

were

the

first that

presented

a real

time

performance

algorithm.

They

used the

Raghavan

and

Roth's

formulation

solving

the

16-

degree

polynomial

by

transforming

this

in

an

eigenvalue

problem.

Their

me

thud

was

adopted

in

urder

to

avoid

the

expansion

of

a

symbolic

determinant,

and

the

"ill

conditioning"

problems

in

solving

the

roots

of

a 16-

degree polynomial.

Although

this

method

could

perform

in real

time

and

avoid

finding the

polynomial

roots, it

generated

new

singularities that

could

appear

and

increase

the

time

performance

notoriously.

Recently

Castellet

and

Thomas,

(1997) have

presented

a

new

approach

using

interval methods, but

this solution

doesn't

perform

in

real

time

yet.

This

article

describes

a

new

real

time

performance

algorithm

to calculate

the

inverse

kinematics

of

a 6R

serial manipulator, which

is

more

efficient

than

the

present

ones.

It

uses

the

Raghavan

and

Roth's

formulation

and

determines

the

16-degree

polynomial

by

evaluating

the

determinant

that

generates

it

in 17

different

points

around

the x origin.

Under

the basis

of

doing

real

time

cuntrol

for path

following,

only

one

of

the

16

possible

manipulator

configurations

is required.

The

angles

at

two

consecutive

instants will

be

close

because

the

manipulator

trajectories

don't

suffer

from

rough

variations.

This

physical fact is used to select only

one

of

the

16

polynomial

roots, particularly

the

closest

to the solution at

the

instant. This

new

approach

allows

reducing

dramatically

the

computation

time

of

the

inverse

kinematics

of

robots

with

general

configurations.

It

doesn't

suffer

in its

efficiency

from

symbolic

expansion

because

that

is

done

before

the algorithm

starts

to

work.

Neither

from

"ill

conditioning"

in

solving

the

roots

of

the

polynomial,

as

it

is

shown

in

the

experimental

results.

It

avoids

the

singularities

problem

because

it

doesn't

use

any

matrix

inversion

in

its process.

This

new

method

allows

changing

manipulator

geometry

in

real time.

For

more

of

the

examples

it is

14th World Congress ofTFAC

able to

calculate

solutions with

double

precision

in

25

ms

in

a

PC

Pentium at

66

MHz.

That

represents

a

25%

of

the

performance

time

of

Manocha's

one.

This

algorithm

was

initially

designed

in

(G6mez,1997)

,

where

more

experimental

results

are

shown.

2.

BACKGROUND

In

order

to

model

a

6R

manipulator

it is used the

Denavit-Hartenberg,

(1955)

formalism.

The

transformation

matrix

that

relates

the

position

of

the

joint

i+l

to

joint

i is

Ai

-Si}·'

sp

ac

1

Ci).,

C;J1:

a;s;

Pi

).i

d;

o 0 1

(1)

where

8

i

represents

the

angle

at

the

joint

th,

O:i

is

the

twist

angle

between

the

axes

j,h

and i+

1,

Si

=

sin(

8,),

Ci

::;;

cos(fJ,),

Pi::;;

sin(~),

A;=cos(o:;),

aiis

the

length

of

link

i+

1

and

d

j

is

the

offset

distance

at

joint

i.

For

a

given

6R

manipulator

we

have

ai'

d

j

•

J1;

and Aj, and

the

pose

of

the

end

-

effector

given

by

A/land.

[

Ix

mx

nx

qx

1

A = ly

my

ny

qy

hand

l

z m

z

n

z

qz

o 0 0 1

(2)

The

problem

of

inverse

kinematics

in

a

6R

manipulator

corresponds

to

computing

the

6

angles

e

i

such

that

A]A2A3A4A5A6

::;;

A

hand

(3)

What

equation

(3)

represents

are

two

4x4

matrices.

The

entries

of

the

left

hand

side are functions

of

the

sines

and

cosines

of

8,.

They

also

depends

on

manipulator

geometry

while

the

right

hand

side

matrix

depends

on

the

end

-

effector

pose.

Both

sides

could

change.

Every

time

the

end

- effector

moves,

it

does

the

A""nd

matrix.

So

does

the left

matrix

if

manipulator

geometry

or

joint

angles

are

changed.

Equation

(3)

corresponds

to

12

scalar

equations.

Since

the

first

3

rows

and 3 columns

of

matrix

A, d

are

orthonormal,

only

6

of

the

12

equations

;'e

independent.

So

the

inverse

kinematics

problem

of

a

6R

manipulator

consists

on

solving a

system

of

6

equations

for

6 unknowns. Unfortunately these 6

equations

are

not

linear

in

the

unknowns

and

besides

that;

the

unknowns

are inside trigonometric

functions.

Three

basic

approaches

have

been

done in

order

to

solve

this

system:

continuation

methods,

elimination

methods

and

interval methods.

The

first

is

based

in

homotopy

techniques

to

solve a

system

of

polynomial equations. It

computes

the

solutions

following

paths

in

the

complex

space.

It

is

robust

but

slow

for real time purposes.

The

third is

based

in interval

methods

for solving

systems

of

non

linear equations, it is also robust

but

slow

too.

The

second

tendency,

which

solves

the

system

towards

a

ISBN:

008

0432484

834

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

successive

elimination

of

unknowns,

is

the

basis

of

the

method

presented

in this article.

It

wa~

first

developed

by

Raghavan

and

Roth,

(1990)

and

used

by

Manocha

and

Canny

(1992)

in the fIrst

real

time

algorithm.

Both

methods

arc

revised

below.

3.

RAG

HA

V

AN

AND

ROTH

METHOD

This

section

describes

the

elimination

method

proposed

by

these

authors.

It

consists

on

reducing

the

non

linear

system

to a

16

degree

polynomial

in

tan(8Y2).

Then

solving

the

roots

of

this

polynomial

and

finally

computing

the

other

angles

by

substitution

in the

elimination

steps.

Raghavan

and

Roth

rearrange

the

equation

(3)

as

A3A4AS =A;IA.l]A"andA6]

(4)

Then,

they

choose

the 3

rd

and

4th

columns

in

both

sides

to

obtain 6

of

the

12

equations

which

are

free

of

8

6

•

In

fact, these 6

equations

represent

the orientation

and

the

position

of

the

6

th

reference

system

expressed

towards

3rd,

4t~

and 5

th

references

in

the

left

hand

side

of

(4)

and

expressed

by

the

6

th

, 1

st

and 2

nd

references

in the

right

hand

side

of

(4),

what

they

named

as p

(PI,

P2,

P3), and 1

(1/,

1

2

,

l,),

vectors.

See

(Lee

and

Liang,1988).

After

this first

elimination,

they

have

a

non-linear

system

of

6

equations

and

5

unknowns.

To

obtain

a linear

system

they

took

as

new

unknowns

the

products

of

sines

and

cosines

in

each

side

of

(4)

having

54

5

S4

C

5

5]52

C4

5

S

5]C2

C)5Z

C4

C

S

(5)

A

=B

c]cZ

54

C

4

51

Cl

S5

52

Cs

1

Cz

where

A is a

6x9

matrix,

whose

entries

are functions

of

53

and C3 and B is

6x8.

Now

the

system

(5)

is

linear

but

there are

only

6

equations

for

16

unknowns.

As (5) is

generated

towards

vectors p and

I they

obtain

new

equations

by

different

products

between

them.

Particularly they

get

8

new

equations

from: p.

p,p.

l,px

1,

(p.

p)l-

(2p.l)p.

With

these

new

equations

and

(5)

they obtain

P

(S455'

s4c5,c4s5

'C4CS'

S4,

C

4

,

5S

,csif

=

(6)

Q

(5I

S

2,5I

C

2,C]S2,C

1

C2,SI,C

I

,S2,C2Y

where

P is a

14x9

matrix

whose

entries

are functions

of

S3 and C3 and Q is

14x8.

The

next step is

the

elimination

of

8

1

and

(12

in

(6).

That

is

done

by

pivoting

in the

terms

of

Q

and

translating

the

operations

to the entries

of

P.

14th World Congress ofTFAC

That

allows

the

elimination

of

(1/ and 8

2

in the last 6

rows

of

(6),

leading

to

the

follow

system

(7)

where

I

is

a

6x9

matrix

which

entries

are

functions

of

53

and

C3'

The

is

the

substitution

2x·

I-x·

Si

=

___L')

, C j ::::

~

(8)

l+xt

l+Xj-

where

xi=tan(B,/2)

Then

(7)

is

transformed

into

l;'

( 2 2 2 2 2 2 T-0

X4

X

S

,X4

X

S

,X

4

, X

4

Xs

,X

4

X

S

,X

4

,Xs

,xs,l

-

(9)

where

I'is

a

6x9

matrix

which

entries are

rational

functions

of

X3'

To

clear

the

denominators

of

the

4

first

rows,

they

are

multiplied

by

(1 +

x/).

The

system

(9) is

converted

into a

squared

one

using

dialytic

elimination,

Raghavan

and

Roth

7

arraving

to

where

l;"

is

a 12x12

matrix

ofthe

form

i'(

4~,4'5

,4,~x;,~Xs

,4,X4X;,X4Xs

,X4'~'Xs,l

J;;;0

(la)

As

the

system

(10)

should

have

a solution

different

from

the

trivial,

it

should

be

..

(L

'

L =

o

(11 )

det

(l:

) = 0

(12)

If

this

determinant

is calculated, its

expansion

will

give a

16-degree

polynomial

in

X3'

Using

a

root

solver,

the

values

of

XJ

an;;

calculated

and afterwards,

the

value

of

(13.

Substituting

this

value

in

(IO)

they

get

8

4

and

8

5

,

Then

find 8

1

and 9

2

in (6)

and

8

6

in

(3).

Obviously,

this is

not

a

real

time

performance

method,

because

the

symbolic

expansion

of

determinant

IN

couldn't

be

in

real

time.

However

it

represents

the

foundations

in

the

Manocha's

method

and

also

in

the

algorithm

that

is

presented

in

this

article.

4.

MANOCHA

METHOD

This

method

has

been

developed

in

(Manocha

and

Canny,

1992),

and

(Manocha

and

Zhu,

1994)

and

was

the

first

real

time

inverse

kinematics

algorithm

for

generalized

6R

manipulators.

To

avoid

the

expansion

of

the

determinant

of

IH

in

real

time

they used the

Rhagavan

and

Roth

approach

but

in a

different

way.

They

treat

the

ai,

d;.

f.1i

and

Ai

and

the

A"and

entries

as

symbolic

constants

and

preprocess

the

entries

of

matrices

P

and

Q in

(6)

as

functions

of

this

constants,

being

P

and

Q as

P =P(a;, d

i

, f.Li' Ai' (13) i =3,4,5

Q =

Q(ai,d"pi'

A"

A/lam!) i = 1,2,6

(13)

ISBN:

008

0432484

835

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

This

maintains

the

algorithm

adequate

to

any

kind

of

geometry.

Given

a

particular

6R

manipulator,

they

substitute

the

numerical

values

of

ai,

dj,

J.li

and

Ai

and

the

A'uwd

in

P

and

Q.

The

next step

is

the

elimination

of

e

1

and e

2

,

for

that

purpose,

thcy factorized P

as

(14)

Then

performing

Gaussian

elimination,

pivoting

on

the

entries

of

Q

matrix

and

translating

the

operations

to

the entries

of

P

l

,

P

2

,

P3.

they

create

matrix

Ein

(7)

with the last 6

rows

of

P

and

apply

the trigometrical

substitution

of

(8).

This

time

they

clean

all

the

denominators

of

I"in

(9)

that

allows

a.E

N

of

(10)

that

ean

be

factorized

as

LOO

=

Axi

+

BX3

+ C (15)

Applying

the

restriction

of

(12)

the

problem

leads

to

solve

the

following

equation

in

Xj

det(« +

B~

+

C)

=0

(16)

Manocha

focused

the

solution

through

an

eigenvalue

problem.

He

established

that

the

values

of

X3

that satisfy

(16)

are

the

eigenvalues

of

the

following

matrix

M

=(

_AO-IC

_:-I

B

)

(17)

where

0

and

I are null

and

identity

matrices

respectively.

This

method

also

allows

obtaining

the

values

of

X4

and

X5

through

the

eigenvectors

of

M.

With

Xi',

X4

and

X5.

he

gets

eJ,

(}4

and

(}5

and

the

rest

of

the

angles

in the

same

way

as

Raghavan

and

Roth

method.

When

matrix

A

becomes

singular,

Manocha

proposes

two

alternatives:

solve

a

generalized

eigenvalue

problem

as

M[

-X3M

2

=0

M[

=[~

~J

M2

=(~

~)

(18)

or

make

certain

transformations

to

the variable

X3

till

the

matrix

A

becomes

non

singular.

In

both

cases

the

algorithm

spends

a

lot

of

time,

reducing

its

global

efficiency.

Manocha

leads

the

solution

to

an

eigenvalue

problem

to

avoid

expanding

symbolic

determinants

and

"ill

conditioning"

problems

in

finding

roots

of

a 16-

degree

polynomial.

But

with

his

method

some

singularities

could

also

appear.

Besides

that, this

method

always

find the

whole

16

solutions

to

the

problem,

It

could

seems

positive,

but

if

only

one

of

the

16 is

required,

for

path

following,

there

are a lot

of

resources

wasted.

In

the

new

method

is

proposed

to find

one

of

the

16

possible

solutions

avoiding

singularities.

5.

ALGORITHM

FORMULATION

A

new

method

for

calculating

the

X3

values

in

(16)

is

presented

here.

The

main

objective

is

to

calculate

one

of

the

16

possible

values

in

order

to

control

a

6R

manipulator.

It

is

supposed

that

its

movement

starts

at

one

pose,

where

the 6

joint

angles

are

known

and

it

14th World Congress ofTFAC

doesn't

suffer

great

variations

in

its

movement.

Every

x]

calculated

is

done,

by

using

the

value

of

the

5.1Discussion

The

Manocha

method

avoids

to

calculate

the

polynomial

roots

with a

root

solver

because

the

possible

"ill

conditioning"

problems.

III

conditioned

polynomials

were

first

studied

by

Wilkinson,

(1959),

who

established

that

solving

the

roots

of

polynomials

of

high

degree,

that

is 19

or

higher,

could

be

a

"ill

conditioned"

problem. It

could

happen that a

small

perturbation

in

the

coefficients

of

the

polynomial

brings

great

modifications

in

its

roots.

The

real

time

inverse

kinematics

calculation

of

Manocha

leads

to

find

the

solutions

of

equation

(16),

which

is a

24

degree

polynomiaL

Trying

to

find

the

roots

of

this

polynomial

will

require

a very

high

precision

root

solver

to

avoid

the"

ill

condition"

of

the

polynomial,

and

that

means

time

waste.

This

was

the

main

reason

why

Manocha

method

avoids

root

solving.

Equation

(16)

could

be

solved

in

a

different

way.

avoiding

"ill

conditioning"

problems

and

the

singularities

of

the"

eigenvalue

way".

If

we

name

R(xj)

to

the

polynomial

generated

by

(16),

that

is

R(x3)

=

det(Axi

+ BX3 +

C)

(19)

We

have

that

R (Xl) is

24

degree

polynomial,

but

we

also

have

that

(1 +

X/)

4

divides

it.

As

it

is

shown

in

(Raghavan

and

Roth,

1990)

and

(Manocha

and

Canny,1992).

therefore we

have

a

new

16

degree

polynomial

S(X3)

such

that

R(.\3) =S(.\3)0+

xi)4

(20)

And

we

have

that

2

Sex

) =

det(

Ax

3 +

BX3

+

C)

3

(1

+

X;)4

(2l)

This

new

algorithm

calculates

the

polynomial

S (X3)

by

evaluating

the

expression

(21)

in

17

X3

different

points

around

X,=O.

These

points

should

be

in

an

interval

[-7..8] to

avoid

out

of

range

values

when

evaluating

higher

powers

of

X,

in

S(X3J.

By

this

method

we

get

a square

linear

system

of

17

equations,

where

the

unknowns

are

the 17

coefficients

of

the 16-degree

polynomiaL

Solving

the

system

we

have

the

polynomial.

Now

we

have

a 16-degree

polynomial

that

doesn't

suffer

from

"ill

conditioned"

problems

and

whose

roots

can

be

easily

found.

5.2

Execution

steps

The

algorithm

follows

the

steps:

1.

Following

the

Manocha

method,

we

generate

the

entries

of

matrices

P and Q

of

(6)

as functions

of

the

Denavit-Hartenberg

parameters,

ai,

d

i

.

/.1;

and

A,

and the A

hand

•

using

the

computer

algebra

systcm

MAPLE.

That

is

done

once

for

each

general

6R

manipulator.

2.

Given

a particular

manipulator

geometry,

substitute

the

numerical

values

of

Denavit-

Hartenberg

parameters

in P

and

Q entries.

ISBN:

008

0432484

836

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

3.

Eliminate

the

variables

tJ

1

and

tJ

2

from

(6)

using

Gaussian

elimination.

4.

After

this

elimination,

we

obtain

the

matrices

A,

Band

C

of

(15)

5.

Evaluate

de

expression

(2l)

in

17

different

points

to

obtain

the 17

-coefficient

S

(xJ)

6.

Find

one

of

the

roots

of

S

(x,)

by

a

root

solver,

particularly

the

one

closer

to

the

last

solution

of

the

problem.

6.

IMPLEMENTATION

AND

PERFORMANCE

The

new

algorithm

and

the

Manocha

method

have

been

implemented

to

compare

their

time

performance

efficiency.

Both

algorithms

have

been

created

in C

code,

using

the

Watcom

C

++

compiler

in

order

to

obtain

the fastest

possible

code.

The

functions

obtained

have

been

executed

in a

PC

Pentium

at

66

MHz.

The

gaussian

elimination,

the

eigenvalue

algorithm

and

the

root

solver

comes

from

(Press

et

al

1988).

Given

a

particular

manipulator

geometry

and

a

pose

for

its

end-effector

we

have

calculate

the

solution

of

the

inverse

kinematics

problem

by

the

two

ways,

Manocha

and

rootsolving.

Besides

that,

we

have

implemented

a trajectory

for

the

end

-

effector

by

adding

increments

to

its x position.

In

all the

problems

the

algorithm

takes

about

25

ms

on

average

on

a

PC

Pentium

at

66MHz.

That

is the

25%

of

Manocha

method.

In

some

configurations

matrices

P

and

Q

could

be

range

deficient,

which

leads

to

less

than

16

solutions.

In

this

cases

the

range

of

matrix

a

IHwill

be

evalualed,

and

depending

on

it,

we

will

search

for

8,or

4

degree

polynomial

in

X3

using

the

same

method.

The

rest

of

angles

could

be

calculated

using

the

method

presented

in

(Manocha

and

Zhu,

1994).

6.1

Examples

The

manipulator

chosed

was

presented

in

(Raghavan

and

Roth,

1990)

an

is

determined

by

TABLEl

TABLE

/;

Denavit-Hartenberg

parameters

ora

6r

manipulator

ai

d;

ai

0.8

0.9

20.0

1.2 3.7 0.33

0.33

1.0

45.0

1.8

0.5

81.0

0.6

2.1

12.0

2.2

0.63

100.0

The

A

hand

is

placed

in the

following

pose

[

-.17967 -.9049 -.04723

.2.5880]

A - -.7683 .3857 .4609 -3.104

hand

- .4803 -.1796 -.8584 6.9163

o 0 0 1

14th World Congress ofTFAC

The

polynomial

S(X3) is

of

the

form

S(X3)

=1.0871 +

2.o054x

3

+1.5436x; +7.110

lx~

-

1.2420xj

+ 2.6547

x~

+3.1608x~

-

9.9781x~

+15.834x~

-2.5832x~

+14.1'22x;0 + 9.8013x;1

12

13

14

15

+ 1.7745x3 +4.7533x3 - .21500X3

+.51036x3

+ 1.0000x1

6

Assuming

that

in

the

step

the

solution

found

was

x)=

-0.70

in

the

present

control

step

we

search

for

the

root

closest

to the

before

and

we

get

X3=

-.7271,

this

leads

to

the

of

angles

01

=

13.109720,°

2

=

50.992538,

O}

=

-72.044112

8

4

=-7.1962516,11

5

=37.852304,116

=6.8315183

If

the

24

polynomial

R(X3) is

considered

R(x

3

)

=

+.615ge-l

+ .2544e16x

3

-.9467

e16x;

+

.l558e17x~

-.1

52ge17xj

+

.1014e17x~

-.4870e16x~

+ .1770e16xI

-5010e15x~

+

.1

128e15xr

-.204geI4x~o

+

.3036e13x11-3695e12x~2

+

.3708el1x~3

..

3075elOx~4

+ .2104e9xj5

-.l184e8x~6

+

.5438e6xr

·.2018e5x~g

+S97.3xj9

-13.24x;o

+.6181xi

1

+.2116xi

2

+.2894e-lxi

3

+.5665e-lxj-4

root-solvers

will

meet

the

"ill

conditioning"

problems

and, unless

40

digits

of

precision

are used, the roots

found

will

be

different

from

the real ones.

Any

of

the

16

possible

configurations

can

be

followed.

For

instance,

in

the next

table

it

can

be

seen the

evolution

of

another

configuration,

for the

geometry

and

pose

presented

in

this

example,

while

the

qz

of

the

end

effector

is

changed.

TABLE

//:

Execution

of

one

trajectorie

5.3-0.05*0

-.414214.0

29.6

-44.971.0

-63.0

10.

5.3-0.05*1

-.3585

15.0 25.1

-39.469.2

-70.3 16.

5.3-0.05*2

.3100

16.1

21.0

-34.4

67.5

-76.4

22.

5.3-0.05*3 .2653 17.2 17.0

-29.765.6

-81.7

27.

5.3-0.05*4

-.2226

18.4 13.1 -25.1

63.6

-86.3 31.

5.3-0.05*5

-.180619.6

9.1

-20.461.6

-90.6

35.

5.3-0.05*6

-.1381

20.9

5.0

-15.759.4

-94.4

39.

5.3-0.05*7

-.093622.4

0.6

-10.657.0

-97.5 43.

5.3-0.05*8

-.044724.1

4.3

-5.1

54.2

-101.054.

5.3-0.05*9

.0140

26.2

10.5 1.6

50.9

-lO3.670.

5.3-0.05*10.16677

-1293i

As

it

can

he

seen,

when

the

end

-

effector

reaches

the

position

where

q,

=5.3-0.5,

XJ

gives a

complex

value

with

non-null

imaginary

part.

This

means

that

this

position

is

not

allowed

for

the

end-

effector

with this

geometric

and

pose

constrains.

For

these

examples

the

average

running

time

of

Manocha

method

is

50

ms

while

this

new

method

wasted

only

25

ms.

In

the

follow

example,

it

be

can

seen

a

contiguration

that

makes

singular

the

matrix

A in

(16).

ISBN:

008

0432484

837

A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...

This

implies

the

generalized

eigenvalue

problem

in

Manocha

method.

The

problem

geometry

is

the

same

as

in

TABLE

I.

This

time

The

Ahana

is

placed

in

the

following

pose

A - -.7683 .3857 0.4609 -3.1046

[

-.1796-.9049 -.04723 -2.5880

1

}u.md

-

.4803-.1796

-.8584 6.9163

o 0 0 1

Using

the

method

presented

here,

we

get

the

of

angles

8

1

=13.109720,8

2

=50.992538,8

3

=-179.999999

8

4

=-7.196251,8

5

=

37.852304,8

6

=6.831518

Now

Manocha

method

wastes

100

ms

while

the

new

algorithm

remains

in

25

ms.

7.

CONCLUSIONS

In

this

paper

we

present

a

new

algorithm

for

inverse

kinematics

of

a

general6R

manipulator,

more

efficient

and

robust

that

the

present

ones.

The

algorithm

performs

polynomial

construction

by

evaluating

a

determinant

in

different

points.

Using

a

root

solver,

it finds

the

solution

of

the

problem

avoiding

"ill

conditioning",

because

of

the

low

degree

of

the

polynomial.

The

algorithm

has

been

tested

in a

variety

of

instances.

We

have

also

tested

in

trajectories

generation.

In all

cases

the

running

time

was

of

25

ms

in

a

PC

Pentium

at

66

MHz,

which

represents

an

improvement

of

25%

over

the

presents.

In

those

cases

where

others

slow

down,

because

degenerated

configurations,

8

3

=180.0

this

algorithm

performs

at

the

same

rate.

Execution

times

of

order

of

cents

of

seconds

make

this

algorithm

more

adequate

than

the

present

ones

to

the

real

time

control

of

robots

whose

configurations

are

generalized,

using

low

performance

computer

systems

as

a

PC

Pentium

8.

ACKNOWLEDGMENTS

This

work

has

been

supported

by

the

Spanish

CICYT

under

grant

TAP-96-1028-C02-02.

9.

REFERENCES

Albala

H.

and

Angeles

1.

(1979).

Numerical

solution

to

the

input

output

displacement

equation

of

the

general

7r

spatial

mechanism.

In:

Proceedings

of

the Fifth World Congress

on

Theory

of

Machines

and

Mechanisms.

1008

-101

1.

Castellet

A.

and

Thomas

F.

(1997).

Towards

an

Efficient

Interval

Method

for

Solving

Inverse

Kinematic

Problems.

In:

Proceedings

of

IEEE

Conference

on

Robotics

and

Automation. 3615-

3620.

Denavit

J.

and

Hartenberg

R.S.

(1955).

A

kinematic

notation

for

lower-pair

mechanism

based

upon

matrices.

Joumal

of

applied

Mechanics, 77:215-

221.

Duffy

J.

and

Crane

C.

(1980).

A

displacement

analysis

of

the general

spatial

7r

mechanisms.

In:

Mechanism

and

Machine

Theory

15:153-169

14th World Congress ofTFAC

06mez

S. R.

(1997).

Disefio

de

Algoritmos

para

la

Representacion

Cinematica

de

Solidos

Articulados

en

Entomos

Virtuales.

Aplicaci6n

a

la

Telepresencia.

Ph.D.

thesis,

Universidad

Nacional

de

Educaci6n

a

Distancia.

Lee

H.Y.

and

Liang

C.O.

(1988).

A

new

vector

Theory

for

the

analysis

of

spatial

mechanisms.

In:

Mechanism

and

Machine

Theory, 23(3):209-

217.

Lee

H.Y.

and

Liang

C.G.

(1988).

Displacement

analysis

of

the

general

spatial

7-link

7r

mechanism.

In:

Mechanism

and

Machine

Theory,

23

(3):219-226.

Manocha

D.

And

Canny

J.F. (1992).

Real

time

inverse

Kinematics

of

general

6R

manipulators".

In:

Proceedings

of

IEEE

Conference

on

Robotics

and

Automation. 383-389.

Manocha

D.

and

Zhu

Y.(l994).

A

Fast

Algorithm

and

System

for

the

Inverse

Kinematics

of

General

Serial

Manipulators_

In:

Proceedings

Of

IEEE

Conference

on

Robotics

and

Automation.

3348

-

3353.

Pieper

D. (1968).

The

Kinematics

of

Manipulators

Under

Computer

Control.

Ph.D.

thesis,

Stanford

University

Press

W.,

Flannery

B.,

Teukolsky

S.

and

Vetterling

W.

(1988).

Numerical

Recipes

in C.

Cambrige.

Raghavan

M.

and

Roth

B.

(1990).

A

general

solution

for

the

inverse

kinematics

of

all

series

chains.

Proc.

Of

the

8 Th

CISM-IFTOMM

Symposium

on

Robots

and

Manipu[ator.314 - 320,

Roth

B.,

Rastegar

J.

and

Scheinman

V.

(

1973)

On

the

Design

of

Computer

Controled

Manipulators.

In: On Theory

and

Practice

of

Robots

and

Manipulators.

93-113.

First

CISM

IFToMM

Symposium,

Tsai

L.W.

and

Morgan

A.

P.

(1985).

Solving

the

kinematics

of

the

most

general

six

and

five-

degree-of-freedom

manipulators

by

continuation

methods.

In: Transactions

of

the

ASME,

Journal

of

Mechanisms,

Transmissions

and

Automation

in Design,

107:

189-200.

Wilkinson

J.H.

(1959).

The

evaluation

of

the

zeros

of

ill-conditioned

polynomials.

Parts

i

and

ii.

In:

Numerical

Mathematics. 150-166

and

167-180.

ISBN:

008

0432484

838

Outline